//#include "Wire.h"
const uint8_t IMUAddress = 0x41;
const uint16_t I2C_TIMEOUT = 1000;
static int mn_sensor_I2C_Write(uint8_t reg, uint8_t value)
{
  int ret = -1;
  ret = i2cWrite(reg, value, true);
  if (ret)
    Serial.printf("[%s]: ret=%d \r\n", __func__, ret);
  return ret;
}

static int mn_sensor_I2C_Write_Block(uint8_t reg, uint8_t value[], uint8_t len)
{
  int ret = -1;
  ret =i2cWrite(reg, value, len, false);
  if (ret)
    Serial.printf("[%s]: ret=%d \r\n", __func__, ret);
  return ret;
}
static int mn_sensor_I2C_Read0(uint8_t reg, uint8_t len)
{
  int ret = -1;
  mTask_txBuf[0] = reg;
  ret = mn_sensor_I2C_Read(DEVREG_ALS_STATUS, mTask_txBuf, len);
  if (ret)
    Serial.printf("[%s]: ret=%d \r\n", __func__, ret);
  return ret;
}
uint16_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop)
{
  return i2cWrite(registerAddress, &data, 1, sendStop);
}

uint16_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop)
{
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data, length);
  uint8_t rcode = Wire.endTransmission(sendStop);

  if (rcode)
  {
    Serial.print(F("i2cWrite failed: "));
    Serial.println(rcode);
  }
  return rcode;
}

uint16_t mn_sensor_I2C_Read(uint16_t registerAddress, uint16_t *data, uint8_t nbytes)
{
  uint32_t timeOutTimer;
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  uint8_t rcode = Wire.endTransmission(false);
  if (rcode)
  {
    Serial.print(F("i2cRead failed: "));
    Serial.println(rcode);
    return rcode;
  }
  Wire.requestFrom(IMUAddress, nbytes, (uint16_t)true);
  for (uint16_t i = 0; i < nbytes; i++)
  {
    if (Wire.available())
      data[i] = Wire.read();
    else
    {
      timeOutTimer = micros();
      while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
      if (Wire.available())
        data[i] = Wire.read();
      else
        return 5;
    }
  }
  return 0;
}
